
#include "telemetryparser.h"
#include "telemetryservcon.h"
#include "globals.h"
using namespace TM;
//moved to telemetryservcon.h (in the baseClasses lib to the rest of the TM code)
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ///QDataStream & operator>>(QDataStream &s, TM_ServCon_Frame_t& frame)
// ///QDataStream & operator<<(QDataStream &s, TM_ServCon_Frame_t& frame)
// ///not reading in the sync - this has been done
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
TelemetryParser::TelemetryParser(QObject *parent)
	: QObject(parent)
{
    QSettings settings("inflation.conf",QSettings::IniFormat);
    m_strInFileName   = settings.value("inFile").toString(); 
    m_strOutFileName = settings.value("outFile").toString();
	
    //TODO: check and react if file names are not found
    
    //open the 'raw' data file
    m_fileSource.setFileName ( m_strInFileName );
	if(!m_fileSource.open( QIODevice::ReadOnly )){//closed in the destructor
		qDebug("TelemetryParser::TelemetryParser(): m_fileSource.open() didn't work: file may not be open yet");
	};
	//set the fileInfo
	m_fileInfoSource.setFile ( m_fileSource ) ;

	//open the target data file
    m_fileTarget.setFileName ( m_strOutFileName );
	if(!m_fileTarget.open( QIODevice::WriteOnly | QIODevice::Truncate )){//closed in the destructor
		qDebug("TelemetryParser::TelemetryParser(): m_fileTarget.open() didn't work");
		return;
	};
	//set the fileInfo
	m_fileInfoTarget.setFile ( m_fileTarget ) ;

	//start a m_pollTimer to look at the source regularly
	m_pollTimer = new QTimer(this);
	m_pollTimer->setInterval (300);
	connect(m_pollTimer, SIGNAL(timeout()), this, SLOT(update()));
	m_pollTimer->start();
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
TelemetryParser::~TelemetryParser()
{
	m_fileSource.close();
	m_fileTarget.close();
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void TelemetryParser::update()
{
	static qint64 m_currentPosition = 0;
	static QDateTime lastRead(QDateTime::currentDateTime());
	
	if(!m_fileSource.isOpen()){
		qDebug("TelemetryParser::update(): m_fileSource is not open");
		if(!m_fileSource.open( QIODevice::ReadOnly )){//closed in the destructor
			qDebug("TelemetryParser::update(): m_fileSource.open() couldn't open the dataSource");
			return;
		};
	}
	QDateTime t = m_fileInfoSource.lastModified ();
//	int ss = t.secsTo(lastRead);
	if(m_fileInfoSource.lastModified () > lastRead){// if this lastModified is later than lastRead
		qDebug("TelemetryParser::update(): hasn't been written since last reading: fi.lastModified () < lastRead");
		return;
	}

	//update the last read control variable
	lastRead = m_fileInfoSource.lastModified ();

	if(!m_fileSource.seek(m_currentPosition)){//go to the position where we last read
		qDebug("TelemetryParser::update(): file.seek() didn't work");
		return;
	}

	//read the file
	const qint64 MIN_BYTES = sizeof(TM_ServCon_Frame);
	const qint64 MAX_BYTES = 10000;//what could this be?

	QByteArray newData = m_fileSource.read(MAX_BYTES);//no error checking here
//	int aa = newData.size();
	if(newData.size() < MIN_BYTES){
		//there's probably not a full block
		qDebug("TelemetryParser::update(): newData.size() < MIN_BYTES, waiting ...");
		return;
	}
	else{
		qDebug()<<QString("TelemetryParser::update(): newData.size() = %1").arg(newData.size());
	}
	
	//find the last CRC
	int lastSync = newData.lastIndexOf (SYNC2);
	if(lastSync<0){
		//didn't get a lastSync
		qDebug("TelemetryParser::update(): didn't get a lastSync");
		return;
	}
	int firstSync = newData.lastIndexOf(SYNC1, lastSync-1);
	if(firstSync<0){
		//didn't get a firstSync
		qDebug("TelemetryParser::update(): didn't get a firstSync");
		return;
	}
	if(firstSync==lastSync){
		//only got one sync
		qDebug("TelemetryParser::update(): only got one sync");
		return;
	}

	QByteArray ba(newData.mid(firstSync, lastSync-firstSync +SYNC2.length()));

	QDataStream stream(&ba, QIODevice::ReadOnly);

	TM_ServCon_Frame frame;
	stream >> frame;//read in a block

	//we're finished reading the source - remember where we were
	m_currentPosition = m_fileSource.pos();

	//now get the latest data into the output file

	InflationData inflationData;
	if(!convertToInflationData(frame, inflationData)){
		qDebug("TelemetryParser::update(): condense() failed");
		return;
	}

	//tell the world about the wonderful thing that has happened
	emit dataChanged(inflationData);

	return ;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///	//spit out all the converted values to some sort of struct containing doubles KMS
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
bool TelemetryParser::convertToInflationData(const TM_ServCon_Frame& frame, InflationData& inflationData)
{
// 	frame.sync1[SYNC_LENGTH];
// 	frame.FrameCounter;
	inflationData.inflationTime = frame.Scet*SCET_TIME_CONVERSION;
	
	inflationData.mainTankValveState = frame.Status_Bits&StatusTankValve ;
	inflationData.inflationControlValveState = frame.Status_Bits&StatusInflationControlCommandSequence;
	inflationData.thrusterValveState = frame.Status_Bits&StatusThrusterValve ;
	inflationData.inflationHoseClampState = frame.Status_Bits&StatusHoseClamp;

	 // Pressure sensor values - 2 values per frame because sensors are read with 10 Hz
	inflationData.tankLinePressure = frame.Tank_Line_Pressure [PRESS_LENGTH-1];
	inflationData.feedPressure = frame.Feed_Pressure [PRESS_LENGTH-1];
	inflationData.inflationHosePressure = frame.Hose_Pressure [PRESS_LENGTH-1];
	inflationData.equalizationVolume1Pressure = frame.EV1_Pressure [PRESS_LENGTH-1];
	inflationData.equalizationVolume2Pressure = frame.EV2_Pressure [PRESS_LENGTH-1]; // Equalization Volumes

	 // Accelerometers - 10 values per frame --> 50 Hz
// 	frame.X_Accel [ACCEL_LENGTH-1];
// 	frame.Y_Accel [ACCEL_LENGTH-1];
// 	frame.Z_Accel [ACCEL_LENGTH-1];

	 // Gyros give turn rate around the axis - 10 values per frame --> 50 Hz
// 	frame.X_Rate [RATES_LENGTH-1];
// 	frame.Y_Rate [RATES_LENGTH-1];
// 	frame.Z_Rate [RATES_LENGTH-1];

	 // Temperatures
// 	frame.ServCon_Temp*PCB_TEMP_ONE_BIT_RESOLUTION; // PCBs
// 	frame.PCM_Temp*PCB_TEMP_ONE_BIT_RESOLUTION;
	inflationData.tank1Temperature = frame.Tank_1_Temp*PCB_TEMP_ONE_BIT_RESOLUTION; // Gas Tanks
	inflationData.tank2Temperature = frame.Tank_2_Temp*PCB_TEMP_ONE_BIT_RESOLUTION;
	inflationData.tank3Temperature = frame.Tank_3_Temp*PCB_TEMP_ONE_BIT_RESOLUTION;
//	frame.ICV_Temp;  // Inflation Control Vlave
	inflationData.inflationHoseTemperature = frame.Hose_Temp;  // Balloon Hose
	inflationData.equalizationVolume1Temperature = frame.EV1_Temp;  // Equalization Volumes
	inflationData.equalizationVolume2Temperature = frame.EV2_Temp;
// 	frame.Bat1_Temp*BAT_PACK_ONE_BIT_RESOLUTION;  // Batteries
// 	frame.Bat2_Temp*BAT_PACK_ONE_BIT_RESOLUTION;
// 	frame.Bat3_Temp*BAT_PACK_ONE_BIT_RESOLUTION;

// 	frame.Bat_Voltage; // Battery Voltage
// 	frame.Current;  // Drain Current



	inflationData.missionTimeStamp = frame.Scet*SCET_TIME_CONVERSION;//
	inflationData.inflationStatusMass = 1;
	inflationData.generalStatus = 1;

	return true;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int TelemetryParser::writeFakeData()
{
	//make an fill a fake data structure and serialise it

	static int FrameCounter = 0;
	const double Norm = 100.0;
	double randomNumber = ((double)qrand()/(double)RAND_MAX)*(Norm) - Norm/2; //randon number

	++FrameCounter;
	static QTime time;
	if(time.isNull())//this is just silly way of starting it
		time.start();
	int Scet = time.elapsed()/100;//every hundred milliseconds

	static double pressure = 0.05;
	pressure += FrameCounter;

	static double acceleration = 0.05;
	acceleration += FrameCounter;

	static double rotationRate = 0.05;
	rotationRate += FrameCounter;

	static double temperature = 0.01;
	temperature += FrameCounter;

	static double voltage = 0.01;
	voltage += FrameCounter;
		
	static double current = 0.01;
	current += FrameCounter;

	TM_ServCon_Frame frame;

	for(int i=0; i<SYNC_LENGTH; ++i)
		frame.sync1[i] = SYNC1.toUtf8().at(i);

	frame.FrameCounter = FrameCounter;
	frame.Scet = Scet;
	frame.Status_Bits = StatusLeftBoomDeployed;

	// Pressure sensor values - 2 values per frame because sensors are read with 10 Hz
	for(int i=0; i<PRESS_LENGTH; ++i)
		frame.Tank_Line_Pressure[i] = int(pressure*i / PRESSURE_ONE_BIT_RESOULTION);
	for(int i=0; i<PRESS_LENGTH; ++i)
		frame.Feed_Pressure[i] =  int(pressure*i / PRESSURE_ONE_BIT_RESOULTION);
	for(int i=0; i<PRESS_LENGTH; ++i)
		frame.Hose_Pressure[i] = int(pressure*i / PRESSURE_ONE_BIT_RESOULTION);
	for(int i=0; i<PRESS_LENGTH; ++i)
		frame.EV1_Pressure[i] = int(pressure*i / PRESSURE_ONE_BIT_RESOULTION);
	for(int i=0; i<PRESS_LENGTH; ++i)
		frame.EV2_Pressure[i] = int(pressure*i / PRESSURE_ONE_BIT_RESOULTION);

	// Accelerometers - 10 values per frame --> 50 Hz
	for(int i=0; i<ACCEL_LENGTH; ++i)
		frame.X_Accel[i] = int(acceleration/ACCELERATION_ONE_BIT_RESOULTION);
	for(int i=0; i<ACCEL_LENGTH; ++i)
		frame.Y_Accel[i] = int(acceleration/ACCELERATION_ONE_BIT_RESOULTION);
	for(int i=0; i<ACCEL_LENGTH; ++i)
		frame.Z_Accel[i] = int(acceleration/ ACCELERATION_ONE_BIT_RESOULTION);
	
	// Gyros give turn rate around the axis - 10 values per frame --> 50 Hz
	for(int i=0; i<RATES_LENGTH; ++i)
		frame.X_Rate[i] = int(rotationRate/ RATE_ONE_BIT_RESOULTION);
	for(int i=0; i<RATES_LENGTH; ++i)
		frame.Y_Rate[i] = int(rotationRate/ RATE_ONE_BIT_RESOULTION);
	for(int i=0; i<RATES_LENGTH; ++i)
		frame.Z_Rate[i] = int(rotationRate/ RATE_ONE_BIT_RESOULTION);

	// Temperatures
	frame.ServCon_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION); // PCBs
	frame.PCM_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);
	frame.Tank_1_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION); // Gas Tanks
	frame.Tank_2_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);
	frame.Tank_3_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);
	frame.ICV_Temp = int(temperature/ TEMPERATURE_ONE_BIT_RESOULTION);  // Inflation Control Vlave
	frame.Hose_Temp = int(temperature/ TEMPERATURE_ONE_BIT_RESOULTION);  // Balloon Hose
	frame.EV1_Temp = int(temperature/ TEMPERATURE_ONE_BIT_RESOULTION);  // Equalization Volumes
	frame.EV2_Temp = int(temperature/ TEMPERATURE_ONE_BIT_RESOULTION);
	frame.Bat1_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);  // Batteries
	frame.Bat2_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);
	frame.Bat3_Temp = int(temperature/ PCB_TEMP_ONE_BIT_RESOLUTION);

	frame.Bat_Voltage = int(voltage/ VOLTAGE_ONE_BIT_RESOULTION); // Battery Voltage
	frame.Current = int(current/ VOLTAGE_ONE_BIT_RESOULTION);  // Drain Current

	for(int i=0; i<SYNC_LENGTH; ++i)
		frame.sync2[i] = SYNC2.toUtf8().at(i);


	//make an 'fake' input file
	QFile fakeFile ( m_strInFileName );
	if(!fakeFile.open( QIODevice::Append )){
		qDebug("TelemetryParser::writeFakeData(): m_fileSource.open() didn't work: file may not be open yet");
		return -1;
	};
	QDataStream stream(&fakeFile);
	stream << frame;

	return 0;
}
bool isBitSet(const TM_ServCon_Frame& frame, const ServConTypeMask& mask)
{
	return frame.Status_Bits&mask;
}




/*
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator>>(QDataStream &s, MSensorBlock& sensorBlock)
{
	s >> sensorBlock.sensorId ;//could check for in range
	s >> sensorBlock.blockSize;//could check for in range
	s >> sensorBlock.blockNum;//could check that it's bigger than the last one
	s >> sensorBlock.dataSize;//could check size >0 && <max :expected size (~200)

	sensorBlock.data = new unsigned char[sensorBlock.dataSize];
	for(int i=0; i<sensorBlock.dataSize; ++i){
		s >> sensorBlock.data[i];
	}

	return s;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator>>(QDataStream &s, MTMBlockHeader& header)//use this
{
	for(int i=0; i<SYNC_SIZE; ++i){
		s >> header.sync[i];
	}
	s >> header.blockSize;
	s >> header.crc;//this must be some constant ? can check 
	s >> header.blockNum ;//could check that it's bigger than the last one
	s >> header.blockType ;//could check for in range
	s >> header.timeStamp ;//could check for increasing time

	return s;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator>>(QDataStream &s, MTMBlock& block)
{
	s >> block.header;

	for(int i=0; i<SERVCON_FIXED; ++i){
		s >> block.fixed[i];
	}
	s >> block.dynSize;//could check size >0 && <max :expected size (~200)
	
	for(int i=0; i<block.dynSize; ++i){
		s >> block.dynamic[i];
	}

	return s;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator<<(QDataStream &s, MSensorBlock& sensorBlock)
{
	s << sensorBlock.sensorId ;//could check for in range
	s << sensorBlock.blockSize;//could check for in range
	s << sensorBlock.blockNum;//could check that it's bigger than the last one
	s << sensorBlock.dataSize;//could check size >0 && <max :expected size (~200)

//	stream.readBytes((char*)(in.data), in.dataSize);//what's this??
	for(int i=0; i<sensorBlock.dataSize; ++i){
		s << sensorBlock.data[i];
	}

	return s;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator<<(QDataStream &s, MTMBlockHeader& header)//use this
{
	s << header.blockNum ;//could check that it's bigger than the last one
	s << header.blockType ;//could check for in range
	s << header.timeStamp ;//could check for increasing time
//	stream.readBytes(in.sync, SYNC_SIZE);//problem here as above
	for(int i=0; i<SYNC_SIZE; ++i){
		s << header.sync[i];
	}
	return s;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
///
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QDataStream & operator<<(QDataStream &s, MTMBlock& block)
{
	s << block.header;

	for(int i=0; i<SERVCON_FIXED; ++i){
		s << block.fixed[i];
	}
	s << block.dynSize;//could check size >0 && <max :expected size (~200)

	for(int i=0; i<block.dynSize; ++i){
		s << block.dynamic[i];
	}

	return s;
}
*/